#include "action.h"

ActionCommand action = NONE;

// 站立姿态
void SG90_Stand_Up(void)
{
    Servo_Set_Target_Angle(0, 90);  // 所有腿部保持90度
    delay_ms(50);
    Servo_Set_Target_Angle(1, 90);
    delay_ms(50);
    Servo_Set_Target_Angle(2, 90);
    delay_ms(50);
    Servo_Set_Target_Angle(3, 90);
    delay_ms(50);
}

// 趴下姿态
void SG90_Get_Down(void)
{
    Servo_Set_Target_Angle(0, 40);  // 腿部角度调整实现趴下
    delay_ms(100);
    Servo_Set_Target_Angle(1, 140);
    delay_ms(100);
    Servo_Set_Target_Angle(2, 140);
    delay_ms(100);
    Servo_Set_Target_Angle(3, 40);
    delay_ms(100);
}

// 前进姿态
void SG90_Advance(void)
{
    // 第一步：抬起左前腿和右后腿（对角腿抬起）
    Servo_Set_Target_Angle(0, 60);   // 左前腿抬起并向前准备（原90→60）
    delay_ms(80);  // 增加延时让动作更稳定
    Servo_Set_Target_Angle(1, 100);  // 右后腿抬起并向前准备（原80→100）
    delay_ms(80);
    Servo_Set_Target_Angle(2, 90);   // 右前腿保持支撑
    delay_ms(30);
    Servo_Set_Target_Angle(3, 90);   // 左后腿保持支撑
    delay_ms(30);

    // 第二步：抬起的腿向前摆动
    Servo_Set_Target_Angle(0, 30);   // 左前腿向前摆动（幅度加大：60→30）
    delay_ms(80);
    Servo_Set_Target_Angle(1, 130);  // 右后腿向前摆动（幅度加大：100→130）
    delay_ms(80);
    Servo_Set_Target_Angle(2, 90);   // 右前腿保持支撑
    delay_ms(30);
    Servo_Set_Target_Angle(3, 90);   // 左后腿保持支撑
    delay_ms(30);

    // 第三步：放下抬起的腿
    Servo_Set_Target_Angle(0, 60);   // 左前腿放下（回到抬起位置）
    delay_ms(80);
    Servo_Set_Target_Angle(1, 100);  // 右后腿放下（回到抬起位置）
    delay_ms(80);
    Servo_Set_Target_Angle(2, 90);   // 右前腿保持
    delay_ms(30);
    Servo_Set_Target_Angle(3, 90);   // 左后腿保持
    delay_ms(30);

    // 第四步：抬起右前腿和左后腿（另一对角腿）
    Servo_Set_Target_Angle(0, 60);   // 左前腿现在作为支撑
    delay_ms(30);
    Servo_Set_Target_Angle(1, 100);  // 右后腿现在作为支撑
    delay_ms(30);
    Servo_Set_Target_Angle(2, 120);  // 右前腿抬起（原120保持不变）
    delay_ms(80);
    Servo_Set_Target_Angle(3, 60);   // 左后腿抬起（原80→60，增大反向幅度）
    delay_ms(80);

    // 第五步：抬起的腿向前摆动
    Servo_Set_Target_Angle(0, 60);   // 左前腿支撑
    delay_ms(30);
    Servo_Set_Target_Angle(1, 100);  // 右后腿支撑
    delay_ms(30);
    Servo_Set_Target_Angle(2, 150);  // 右前腿向前摆动（幅度加大：120→150）
    delay_ms(80);
    Servo_Set_Target_Angle(3, 30);   // 左后腿向前摆动（幅度加大：60→30）
    delay_ms(80);

    // 第六步：放下抬起的腿
    Servo_Set_Target_Angle(0, 60);   // 左前腿支撑
    delay_ms(30);
    Servo_Set_Target_Angle(1, 100);  // 右后腿支撑
    delay_ms(30);
    Servo_Set_Target_Angle(2, 120);  // 右前腿放下
    delay_ms(80);
    Servo_Set_Target_Angle(3, 60);   // 左后腿放下
    delay_ms(80);

    // 第七步：所有腿回到中立位置，准备下一步
    Servo_Set_Target_Angle(0, 90);   // 回到中立位置
    delay_ms(50);
    Servo_Set_Target_Angle(1, 90);
    delay_ms(50);
    Servo_Set_Target_Angle(2, 90);
    delay_ms(50);
    Servo_Set_Target_Angle(3, 90);
    delay_ms(50);
}

// 跳舞动作
void SG90_Dance(void)
{
    // 实现复杂的舞蹈动作序列
    // 通过多个舵机的协调运动实现
}


// 后退函数 - 与前进逻辑相反
void SG90_Backward(void)
{
    // 第一步：抬起左前腿和右后腿（对角腿抬起）
    Servo_Set_Target_Angle(0, 120);  // 左前腿向后抬起（前进是60，后退是120）
    delay_ms(80);
    Servo_Set_Target_Angle(1, 60);   // 右后腿向后抬起（前进是100，后退是60）
    delay_ms(80);
    Servo_Set_Target_Angle(2, 90);   // 右前腿保持支撑
    delay_ms(30);
    Servo_Set_Target_Angle(3, 90);   // 左后腿保持支撑
    delay_ms(30);

    // 第二步：抬起的腿向后摆动
    Servo_Set_Target_Angle(0, 150);  // 左前腿向后摆动（前进是30，后退是150）
    delay_ms(80);
    Servo_Set_Target_Angle(1, 30);   // 右后腿向后摆动（前进是130，后退是30）
    delay_ms(80);
    Servo_Set_Target_Angle(2, 90);   // 右前腿保持支撑
    delay_ms(30);
    Servo_Set_Target_Angle(3, 90);   // 左后腿保持支撑
    delay_ms(30);

    // 第三步：放下抬起的腿
    Servo_Set_Target_Angle(0, 120);  // 左前腿放下
    delay_ms(80);
    Servo_Set_Target_Angle(1, 60);   // 右后腿放下
    delay_ms(80);
    Servo_Set_Target_Angle(2, 90);   // 右前腿保持
    delay_ms(30);
    Servo_Set_Target_Angle(3, 90);   // 左后腿保持
    delay_ms(30);

    // 第四步：抬起右前腿和左后腿（另一对角腿）
    Servo_Set_Target_Angle(0, 120);  // 左前腿现在作为支撑
    delay_ms(30);
    Servo_Set_Target_Angle(1, 60);   // 右后腿现在作为支撑
    delay_ms(30);
    Servo_Set_Target_Angle(2, 60);   // 右前腿向后抬起（前进是120，后退是60）
    delay_ms(80);
    Servo_Set_Target_Angle(3, 120);  // 左后腿向后抬起（前进是60，后退是120）
    delay_ms(80);

    // 第五步：抬起的腿向后摆动
    Servo_Set_Target_Angle(0, 120);  // 左前腿支撑
    delay_ms(30);
    Servo_Set_Target_Angle(1, 60);   // 右后腿支撑
    delay_ms(30);
    Servo_Set_Target_Angle(2, 30);   // 右前腿向后摆动（前进是150，后退是30）
    delay_ms(80);
    Servo_Set_Target_Angle(3, 150);  // 左后腿向后摆动（前进是30，后退是150）
    delay_ms(80);

    // 第六步：放下抬起的腿
    Servo_Set_Target_Angle(0, 120);  // 左前腿支撑
    delay_ms(30);
    Servo_Set_Target_Angle(1, 60);   // 右后腿支撑
    delay_ms(30);
    Servo_Set_Target_Angle(2, 60);   // 右前腿放下
    delay_ms(80);
    Servo_Set_Target_Angle(3, 120);  // 左后腿放下
    delay_ms(80);

    // 第七步：所有腿回到中立位置，准备下一步
    Servo_Set_Target_Angle(0, 90);   // 回到中立位置
    delay_ms(50);
    Servo_Set_Target_Angle(1, 90);
    delay_ms(50);
    Servo_Set_Target_Angle(2, 90);
    delay_ms(50);
    Servo_Set_Target_Angle(3, 90);
    delay_ms(50);
}

void SG90_Sit_Down(void)
{
    Servo_Set_Target_Angle(0, 90);   // 左前腿中立
    Servo_Set_Target_Angle(1, 150);   // 右后腿弯曲
    Servo_Set_Target_Angle(2, 90);   // 右前腿中立  
    Servo_Set_Target_Angle(3, 30);  // 左后腿弯曲
    delay_ms(500);  // 保持坐姿
}

// 握手函数 - 左前腿握手3次
void SG90_Shake_Hand(void)
{
    // 准备姿势：先坐下
    SG90_Sit_Down();
    delay_ms(10);
    
    // 抬起左前腿（伸手）
    Servo_Set_Target_Angle(0, 30);   // 左前腿抬起
    delay_ms(200);
    
    // 放下左前腿（握手）
    Servo_Set_Target_Angle(0, 100);  // 左前腿放下
    delay_ms(200);

    // 抬起左前腿（伸手）
    Servo_Set_Target_Angle(0, 30);   // 左前腿抬起
    delay_ms(200);    
}